#pragma once

#include <mutex>
namespace apollo {
namespace drivers {
namespace livox {

template <typename T>
class CircularQueueBuffer {
 public:
  CircularQueueBuffer(unsigned int len) {
    len_ = len + 1;  // use a void block to show that the buffer is full
    buffer_ = new T[len_];
    mtx_.unlock();
    front_ = 0;
    rear_ = 0;
  }

  ~CircularQueueBuffer() { delete[] buffer_; }

  /// @brief Insert data into ring buffer, success is guaranteed
  /// @param input data to be inserted
  /// @return if buffer is not full, insert data and return true. if buffer is
  /// full, drop the oldest data, insert new and return false
  bool Insert(T* input) {
    std::lock_guard<std::mutex> lock(mtx_);
    buffer_[front_].CopyFrom(*input);
    front_ = (front_ + 1) % len_;
    if (front_ == rear_) {
      rear_ += 1;
      rear_ %= len_;
      return false;
    } else {
      return true;
    }
  }

  bool Get(T* output) {
    std::lock_guard<std::mutex> lock(mtx_);
    if ((front_ == rear_)) {
      return false;
    }
    output->CopyFrom(buffer_[rear_]);
    rear_ = (rear_ + 1) % len_;
    return true;
  }

 private:
  std::mutex mtx_;
  T* buffer_;
  unsigned int len_;
  int front_;
  int rear_;
};

}  // namespace livox
}  // namespace drivers
}  // namespace apollo